/*
 *  Copyright (C) 2022  贺龙宇
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include "Protocol.h"




Protocol::Protocol(SerialPort& serial_port)
    : serial_port_(serial_port)
{
}

Protocol::~Protocol()
{
}

void Protocol::sendTarget(cv::Point3f& target)
{
    // static int start_time=std::clock();
    static int cnt=0;
    Targets t { target.x, target.y, target.z };
    unsigned char uart_data[16];
    uart_data[0] = 0x03;
    uart_data[1] = 0xFC;
    memcpy(uart_data + 2 * sizeof(uchar), &t, sizeof(t));
    uart_data[14] = 0xFC;
    uart_data[15] = 0x03;
    serial_port_.Write(uart_data, sizeof(uart_data));
    cnt++;
    if(cnt==2000){
        // printf("send %d datas in one second\n",cnt);
        FYTLog->writeLog("send "+std::to_string(cnt)+" datas in ten seconds");
        cnt=0;
        // start_time=std::clock();
    }
}

McuData mcu_data = {
    0,//id
    EnemyColor::RED,//enemy color。如果在无串口设备中调试时，需以hand_data的值为准
    State::ARMOR_STATE,//state
    15,//shoot_speed
    0,//gimbal_yaw
    0//gimbal_pitch
};

HandData hand_data={
    0,//ID
    EnemyColor::BLUE //enemy color。在无串口设备中调试时，修改颜色时应修改这个值
};

GimbalData gimbal_data={
    0,
    0
};

int serial_states=SERIAL_NORMAL;

bool serial_first_receive=false;

void Protocol::receiveData()
{
    uint8_t buffer[256];
    bool init = false;
    memset(buffer,0,sizeof(buffer));
    while(!init){ 
	    static int index=0;
	    static bool flag=false;
        //读取12字节数据（一组），存入缓存区
	    serial_port_.Read(buffer+12*index,12);
        
        for(int i=0;i<=12*(index-1)+12;i++){
            //在缓存区中寻找帧头帧尾，如果找到则把对应数据copy到mcu_data中
            if(buffer[i]==0x03 && buffer[i+1]==0xFC &&
		    buffer[sizeof(mcu_data)+i+2]==0xFC && buffer[sizeof(mcu_data)+i+3]==0x03){
	            memcpy(&mcu_data,buffer+i+2,sizeof(mcu_data));
                if(!serial_first_receive){
                    serial_first_receive=true;
                }
	            index=0;
	            flag=true;
	            break;
	        }

            //若在缓冲区中未找到帧头帧尾，则重新读取一组数据，追加保存在buffer后面
            //如果buffer中存放超过10组数据，则重置buffer（index=0）
	        else if(i==(index-1)*12+12){
		        index++;
		        flag=false;
		        //printf("fail\n");
	            if(index>10) index=0;
		        break;
	        }
        }
	    if(!flag) continue;

        
         
        //电机偏航位置连续化处理 电机
        static int circle_cnt=0,last_yaw=0;
        //此处的跳变情况：8000+ -> 1000- 应加上8192
        if(last_yaw-mcu_data.gimbal_yaw>7000){
            circle_cnt++;
        }
        //另一种跳变情况：1000- -> 8000+ 应减去8192
        else if(mcu_data.gimbal_yaw-last_yaw>7000){
            circle_cnt--;
        }

        //将转换好的云台数据保存到gimbal_data对象中
        gimbal_data.pitch=mcu_data.gimbal_pitch;
        gimbal_data.yaw=mcu_data.gimbal_yaw+circle_cnt*8192;
        last_yaw=mcu_data.gimbal_yaw;

        //串口V2.0：将数据写入HandData中
        hand_data.enemy_color=mcu_data.color;
        hand_data.id=mcu_data.id;

        //写日志
	    static int cnt=0;
        cnt++;
        if(cnt==100){
            FYTLog->writeLog("Recieve the serial data:"+std::to_string((int)(mcu_data.state))+" "
            +std::to_string(mcu_data.shoot_speed)+" "+std::to_string(mcu_data.gimbal_yaw)+" "
            +std::to_string(mcu_data.gimbal_pitch));
            cnt=0;
        }
	    //printf("%d:",cnt);
        //printf("receive the serial data\n %d,%d,%d,%d,%d,%d\n",
        //  mcu_data.id,mcu_data.color,
        //   mcu_data.state,mcu_data.shoot_speed,mcu_data.gimbal_yaw,mcu_data.gimbal_pitch);
            
            // printf("gimbal data:%d %d\n",gimbal_data.yaw,gimbal_data.pitch);
            //memset(buffer,0,sizeof(buffer));
        // usleep(50000);
    }
}
